#!/usr/bin/env python
import roslib; roslib.load_manifest('test_turtlebot')
import rospy
import tf
import math
import sys
from std_msgs.msg import String
from sensor_msgs.msg import LaserScan   # laser scans
from geometry_msgs.msg import Twist     # for sending velocity commands
from geometry_msgs.msg import Vector3   # for sending velocity commands

_lx = 1
_ly = 0 
_lz = 0 
_ax = 0 
_ay = 0 
_az = 0	

def treatSensorData(data):
	global _lx, _ax, _az

	minrange = min(data.ranges)

	if(minrange < data.range_max/10 and minrange>data.range_min/10 and data.ranges.index(minrange) > 0):
		rospy.loginfo("Obstable detecte")
		_lx = 0
		_az = -1
	elif(minrange < data.range_max/10 and minrange>data.range_min/10 and data.ranges.index(minrange) <= 0):
		_lx = 0
		_az = 1
	else:
		_lx = 1
		_az = 0

def run():
	pub = rospy.Publisher("cmd_vel", Twist)
	rospy.Subscriber("scan", LaserScan, treatSensorData)
	rospy.init_node("firstnode")
	str = "Demarrage"
	rospy.loginfo(str)


	#rate = rospy.Rate(1.0)

	while not rospy.is_shutdown():
		twist = Twist()	
		twist.linear.x = _lx           
		twist.linear.y = _ly
		twist.linear.z = _lz
		twist.angular.x = _ax
		twist.angular.y = _ay
		twist.angular.z = _az
		pub.publish(twist)
		#rate.sleep()


if __name__ == '__main__':
	try:
		run()
	except rospy.ROSInterruptException: pass

	

